﻿using System;
using System.Collections.Generic;
using System.Linq;
using System.Text;

namespace Robot.SearchNearest
{
    class State
    {
        internal Bram.NxtSharp.NxtBrick brick = null;
        internal StateHandler handler = null;
        public StateHandler Handler
        {
            get { return handler; }
            set { handler = value; }
        }
        public Logger Logger
        {
            get { return handler.Logger; }
            set { handler.Logger = value; }
        }
        public State(Bram.NxtSharp.NxtBrick brick)
        {
            this.brick = brick;
        }
        internal virtual void _Start() { }
        public virtual void Start() { }
        internal virtual void _Tick() { }
        public virtual void Tick() { }
        internal virtual void _End() { }
        public virtual void End() { }
    }

    class TimedState : State
    {
        private System.Diagnostics.Stopwatch watch = new System.Diagnostics.Stopwatch();
        public long ElapsedTime
        {
            get { return watch.ElapsedMilliseconds; }
        }

        private long limit;

        public TimedState(Bram.NxtSharp.NxtBrick brick, long limit)
            : base(brick)
        {
            this.limit = limit;
        }

        internal override void _Start()
        {
            watch.Reset();
            watch.Start();
        }

        internal override void _Tick()
        {
            if (ElapsedTime >= limit)
            {

            }
        }
    }

    class RunningState : State
    {
        private int speed;
        private int distance;
        public RunningState(Bram.NxtSharp.NxtBrick brick, int speed, int distance)
            : base(brick)
        {
            this.speed = speed;
            this.distance = distance;
        }

        public override void Start()
        {
            brick.MotorB.Turn(speed, distance);
            brick.MotorC.Turn(speed, distance);
        }

        public override void End()
        {
            brick.MotorB.Brake();
            brick.MotorC.Brake();
        }
    }

    class StoppedState : State
    {
        public StoppedState(Bram.NxtSharp.NxtBrick brick)
            : base(brick)
        {
        }

        public override void Start()
        {
            brick.MotorB.Brake();
            brick.MotorC.Brake();
        }
    }

    class SearchState : State
    {
        int degrees;
        int speed;
        int factor = 700;
        int minDistance = 256;
        int minIndex = -1;
        StateHandler move = new StateHandler();
        public SearchState(Bram.NxtSharp.NxtBrick brick, int degrees, int speed)
            : base(brick)
        {
            this.degrees = degrees;
            this.speed = speed;
        }

        public override void Start()
        {
            int steps = 359 / degrees + 1;
            for (int i = 0; i < steps; ++i)
            {
                brick.Sensor4.Poll();
                Logger.Log("Value at " + i + ": " + brick.Sensor4.RawValue.ToString());
                if (brick.Sensor4.RawValue < minDistance)
                {
                    minDistance = brick.Sensor4.RawValue;
                    minIndex = i;
                }
                brick.MotorB.Turn(speed, degrees * 4);
                System.Threading.Thread.Sleep(factor * degrees / speed);
            }
            brick.MotorB.Turn(speed, (minIndex * degrees * 19 / 20) * 4);
            System.Threading.Thread.Sleep(factor * minIndex * degrees / speed);
            handler.CurrentState = new RunningState(brick, speed, minDistance * 15);
        }
    }

    class StateHandler
    {
        private State currentState;
        public State CurrentState
        {
            get { return currentState; }
            set
            {
                if (currentState != null)
                {
                    currentState.End();
                    currentState._End();
                }
                currentState = value;
                if (currentState != null)
                {
                    currentState.Handler = this;
                    currentState._Start();
                    currentState.Start();
                }
            }
        }

        internal Logger logger = null;
        public Logger Logger
        {
            get { return logger; }
            set { logger = value; }
        }

        public void Tick()
        {
            currentState._Tick();
        }
       
    }
}
